#include "stdafx.h"

#include "world.h"
#include "collisionsolving.h"

LinkedList<Spring *> *World::mSprings = 0;
LinkedList<Body   *> *World::mBodies  = 0;
std::vector<Shape  *> *World::mShapes  = 0;

void World::Init()
{
    mSprings = new LinkedList<Spring *>();
    mBodies  = new LinkedList<Body   *>();
    mShapes  = new std::vector<Shape  *>();
}

void World::Free()
{
    delete mSprings;
    delete mShapes;
    delete mBodies;
}

void World::Tick()
{
    // Update shapes (collision detection)
    /*int n = -1, s = mShapes->size();
    for( int i = 0 ; i < s ; ++i )
    {
        for( n++ ; n < s ; ++n )
        {
            CollisionSolving::Collide( (CircleShape *) mShapes->at( i ), (CircleShape *) mShapes->at( n ) );
        }
    }*/

    // Update springs
    do
    {
        Spring *spring = mSprings->GetNext();

        float length  = (spring->mFrom->mPosition - spring->mTo->mPosition).Magnitude();
        float ilength = 1 / length;
        float ldiff   = length - spring->mRestLength;
        float dx      = spring->mFrom->mPosition.x - spring->mTo->mPosition.x;
        float dy      = spring->mFrom->mPosition.y - spring->mTo->mPosition.y;

        float compX, compY;

        compX =  spring->mSpringConstant * ldiff;
        compX += spring->mDampingConstant * (spring->mFrom->mVelocity.x - spring->mTo->mVelocity.x) * dx * ilength;
        compX *= -dx * ilength;

        compY =  spring->mSpringConstant * ldiff;
        compY += spring->mDampingConstant * (spring->mFrom->mVelocity.y - spring->mTo->mVelocity.y) * dy * ilength;
        compY *= -dy * ilength;

        spring->mFrom->mVelocity.AddX( compX );
        spring->mFrom->mVelocity.AddY( compY );
        spring->mTo->mVelocity.AddX( -compX );
        spring->mTo->mVelocity.AddY( -compY );
    } while( mSprings->HasNext() );

    // Update bodies
    do
    {
        Body *body = mBodies->GetNext();

        body->mPosition += body->mVelocity;
    } while( mBodies->HasNext() );
}

Shape *World::ShapeNew( Vector2 pPosition )
{
    // Keep the bodies private, don't want direct position changes
    Body *body = new Body( pPosition );

    Shape *shape = new Shape( body );

    mShapes->push_back( shape );
    mBodies->Add( body );

    return shape;
}

CircleShape *World::CircleShapeNew( Vector2 pPosition, float pRadius )
{
    // Keep the bodies private, don't want direct position changes
    Body *body = new Body( pPosition );

    CircleShape *shape = new CircleShape( body, pRadius );

    mShapes->push_back( shape );
    mBodies->Add( body );

    return shape;
}

Spring *World::SpringNew( Shape *pFrom, Shape *pTo, float pSpringConstant, float pDampingConstant, float pRestLength )
{
    Spring *spring = new Spring( pFrom->mBody, pTo->mBody, pSpringConstant, pDampingConstant, pRestLength );

    mSprings->Add( spring );

    return spring;
}